clear all ; close all ;


ggg = dir('*_Pleth_PTT.txt') ;
addpath('/Users/hautiengwu/Desktop/dsSTFT_code/CodeGeneric') ;

Hz = 4 ;
basicTF.win = 100*4*10 ; %4096;
basicTF.hop = 20; %441;
basicTF.fs = Hz;
basicTF.fr = 2e-4;
basicTF.feat = 'SST11';
advTF.num_tap = 1;%num_tap(cc);
advTF.win_type = 'Gauss'; %{'Gaussian','Thomson','multipeak','SWCE'}; %}
advTF.Smo = 1;
advTF.Rej = 0;
advTF.ths = 1E-9;
advTF.HighFreq = 1.5/50;
advTF.LowFreq = 0.005/50;
advTF.lpc = 0;
cepR.g = 0.2 ;
cepR.Tc=0;
P.num_s = 1;
P.num_c = 1;





fprintf(['Case NO = ',num2str(length(ggg)),'\n']) ;
LEN = zeros(length(ggg),1) ;


for caseNO = 1:length(ggg)        
    [x0] = dlmread(ggg(caseNO).name) ;
    LEN(caseNO) = size(x0,1) ;
    
    sig = x0(:,2)-median(x0(:,2)) ;
    trend = zeros(size(sig)) ;
    for jj =1 :length(trend)
        idx = max(1, jj-100*4) : min(length(sig), jj+100*4) ;
        trend(jj) = median(sig(idx)) ;
    end
    Jidx = find(abs(sig-trend)<25) ;
    sig2 = interp1(Jidx, sig(Jidx)-trend(Jidx), [1:length(sig)], 'linear', 'extrap') ;

    ALLsig2{caseNO} = sig2 ;
    ALLsig{caseNO} = sig-trend ;
    ALLtrend{caseNO} = trend ;
end


goodCASE = 1:36 %[1:11 14:16 18 23:24 26 28:30 33 35:38] ;
% LAG = [] ;
% for ii=2:length(goodCASE)
%     tmp = [] ; 
%     for jj=1:400
%         tmp2 = ALLsig2{goodCASE(ii)}(jj:4800) ; 
%         tmp3 = ALLsig2{1}(1:4800-jj+1) ; 
%         tmp(jj) = sqrt(sum(tmp2-tmp3).^2/length(tmp2)) ;
%     end
%     [~,b] = min(tmp) ; LAG(ii) = b(1) ;
% end
% 
% 
% SIG = zeros(348, 20) ;
% for jj=1:20 ; idx = round((jj-1)*4/xi(23))+1: round((jj-1)*4/xi(23))+348 ; SIG(:,jj) = ALLsig2{1}(idx); end
% new1=zeros(size(ALLsig{1})) ;
% for jj=1:20 ; idx = round((jj-1)*4/xi(23))+1: round((jj-1)*4/xi(23))+348 ; new1(idx) = median(SIG,2) ; end

%% plot signal
figure
for ii = 1:length(ggg) ; 
    plot([1:4:length(ALLsig{ii})]/4/60, ALLsig{ii}(1:4:end)+ALLtrend{ii}(1:4:end)+(ii-1)*30) ; hold on; 
end
axis([-inf inf -30 1000]) ; set(gca,'fontsize', 24) ;
xlabel('Time (min)') ; ylabel('a.u.') ;
export_fig(['AllSignalFigure'],'-transparent','-pdf','-nocrop');



%% plot spectrum
figure
for ii=1:length(ggg)
    xi = 2*[1:(LEN(ii)-1)/2]./((LEN(ii)-1)/2);
    xhat = fft(ALLsig2{ii}) ; 
    [a,b]=max(abs(xhat(2:(LEN(ii)-1)/2)));
    PEAK(ii) = 1./xi(b(1)) ; 
    plot(xi, 1e-4*(abs(xhat(2:(LEN(ii)-1)/2+1))+(ii-1)*.7e4)) ; hold on; 
end 
axis([0 0.12 -inf 24.5] );
plot([0.01 0.01], [0 24.5], 'k--', 'linewidth',2)
plot([0.095 0.095], [0 24.5], 'r--', 'linewidth',2)
set(gca,'fontsize', 24) ;
xlabel('Frequency (Hz)'); ylabel('a.u.') ;
export_fig(['AllSignalFigurePS'],'-transparent','-pdf','-nocrop');




%% dsSST

MEANdeShapeSTFT = zeros(600, 202) ;
MEANdeShapeSST = zeros(600, 202) ;

for caseNO = 33:length(ggg)
    
    disp(caseNO) ;
    
    [x0] = dlmread(ggg(caseNO).name) ;
    LEN(caseNO) = size(x0,1) ;
    
    sig = x0(:,2)-median(x0(:,2)) ;
    trend = zeros(size(sig)) ;
    for jj =1 :length(trend)
        idx = max(1, jj-100*4) : min(length(sig), jj+100*4) ;
        trend(jj) = median(sig(idx)) ;
    end
    sig0 = sig-trend ;
    sig = sig-trend ;
    
    for jj = 1:length(sig)
        idx = max(1, jj-60*4) : min(length(sig), jj+60*4) ;
        if sig(jj)>quantile(sig(idx), .92) * 2 ;
            sig(jj) = median(sig) ;
        elseif sig(jj)<quantile(sig(idx), .05)- 10;
            sig(jj) = median(sig) ;
        end
    end
    
    x = sig ; x(find(x>50)) = 50 ;
    t = [1:length(sig)]/ Hz ;
    disp(length(x)) ;
    
    if length(x)<6001
        fprintf('TOO Short signal\n') ;
    else
        
        time1 = tic ;
        [~, ~, ~, deShapeSTFT, deShapeSST, tfrsq, tfrtic] = CFPH(x+1, basicTF, advTF, cepR, P);
        toc(time1) 
    
        time_stamp = basicTF.hop/basicTF.fs;
    
        figure ; 
        imageSQ(0:time_stamp:t(end), tfrtic*basicTF.fs, abs(deShapeSST), 0.995); axis xy; colormap(1-gray);
        xlabel('time (sec)'); ylabel('Frequency (Hz)'); set(gca,'fontsize',28) ; 
        set(gca,'ytick',[0.01:0.01:0.08]) ; %title('MEAN SST') ;
        yyaxis right
        plot(t, x0(:,2),'b'); axis([-inf inf 100 300]);
        ylabel('PTT (ms)') ;
        export_fig(['PTT_deShapeSTFT_case',num2str(caseNO)],'-transparent','-pdf','-nocrop');
        close 
    
        MEANdeShapeSTFT = MEANdeShapeSTFT + abs(deShapeSTFT(:, 1:202)) ;    
        MEANdeShapeSST = MEANdeShapeSST + abs(deShapeSST(:, 1:202)) ;
    end
    
    clear deShapeSTFT; clear deShapeSST ; clear x ;
end

%{


% subplot(221) ; hold off;
% imageSQ(0:time_stamp:t(end), tfrtic*basicTF.fs, MAXdeShapeSTFT, 0.995); axis xy; colormap(1-gray);
% xlabel('time (sec)'); ylabel('Frequency (Hz)'); set(gca,'fontsize',28) ; title('Max STFT') ;
% 
% subplot(222) ; hold off;
% imageSQ(0:time_stamp:t(end), tfrtic*basicTF.fs, MAXdeShapeSST, 0.995); axis xy; colormap(1-gray);
% xlabel('time (sec)'); ylabel('Frequency (Hz)'); set(gca,'fontsize',28) ; title('Max SST') ;
% 
% subplot(223) ; hold off;
% imageSQ(0:time_stamp:t(end), tfrtic*basicTF.fs, MEANdeShapeSTFT, 0.995); axis xy; colormap(1-gray);
% xlabel('time (sec)'); ylabel('Frequency (Hz)'); set(gca,'fontsize',28) ; title('Mean STFT') ;

%subplot(224) ; hold off;
figure ; imageSQ(0:time_stamp:t(end), tfrtic*basicTF.fs, MEANdeShapeSST, 0.995); axis xy; colormap(1-gray);
xlabel('time (sec)'); ylabel('Frequency (Hz)'); set(gca,'fontsize',28) ; 
set(gca,'ytick',[0.01:0.01:0.12]) ; axis([-inf inf 0 0.12]) ;
%title('MEAN SST') ;

%yyaxis right
%plot(t, x0(:,2),'b'); axis([-inf inf 100 300]);

export_fig('PTT_deShapeSTFT','-transparent','-pdf','-nocrop');
%}




%%
caseNO = 33 ;
    [x0] = dlmread(ggg(caseNO).name) ;
    LEN(caseNO) = size(x0,1) ;
    x0 = x0*2 ;
    
    sig = x0(:,2) ;
    trend = zeros(size(sig)) ;
    for jj =1 :length(trend)
        idx = max(1, jj-100*10) : min(length(sig), jj+100*10) ;
        trend(jj) = median(sig(idx)) ;
    end
    trend=smooth(trend,1000,'loess');
    sig0 = sig-trend ;
    sig = sig-trend ;
    
    for jj = 1:length(sig)
        idx = max(1, jj-60*4) : min(length(sig), jj+60*4) ;
        if sig(jj)>quantile(sig(idx), .92) * 2 ;
            sig(jj) = median(sig) ;
        elseif sig(jj)<quantile(sig(idx), .05)- 10;
            sig(jj) = median(sig) ;
        end
    end

J = reshape(x0(1:8000), 400, 20) ;

J0 = reshape(sig(1:8000), 400, 20) ;
[idx,dist] = knnsearch(J0', J0','k',20) ;
epsilon = median(dist(:,15)) ;
artifact = zeros(size(sig(1:8000))) ;
for ll = 1:20 ;
    tmp = findEstimate(J0(:,idx(ll,:)), exp(-dist(ll,:).^2/epsilon^2)', 1) ;
    artifact((ll-1)*400+1:ll*400) = tmp ;
end

% JJ = repmat(median(J,2),1,20);
% artifact = JJ(:) ;


        figure ; 
        plot(t,x0(:,2),'k') ; 
        xlabel('time (sec)'); ylabel('Original PTT (ms)'); set(gca,'fontsize',28) ; 
        axis([-inf inf 300 600]);
        %set(gca,'ytick',[0.01:0.01:0.08]) ; %title('MEAN SST') ;
        yyaxis right
        plot(t(1:8000), x0(1:8000,2)-artifact-50,'b'); axis([-inf inf 300 600]);
        ylabel('Corrected PTT (ms)') ;
        set(gca,'ytick',2*[150 200 250 300]);
        set(gca,'yticklabel',2*[200 250 300 350]);
        export_fig(['PTT_Recovery_case',num2str(caseNO)],'-transparent','-pdf','-nocrop');